/*  Sketch for our IR Sensor package'
 *
 *  Swings a servo back and forth with the IR sensor 
 *  attached and scans for the objects.
 *
 *  Uses:
 *   - IrSensor class for Sharp GP2D12 IR sensor
 *   - Servo library to controll a servo 
 *
 */
 
#include <Servo.h>
#include "IrSensor.h"
#include <stdlib.h> //atoi
#include <avr/sleep.h>

// This identifies the interrupt number for our chipset:
//   0 = digial pin 2
//   1 = digital pin 3
#define INTERRUPT_NUM 1 

// set up the button pin to match the interrupt number.
#define BUTTON_PIN 3 
// set up the a pin drive the button
#define DRIVER_PIN 4 

// store the current button state this is volatile because 
// the ISR routine will chagnge its value
volatile int button_state = HIGH; 

#define BUTTON_ENABLE LOW

// declare our IR sensor object on Analog 0 pin
IrSensor ir_sensor(A0);

// declare our servo object on Digital pin 11
Servo scan_servo;

#define SERVO_PIN 11

void handleButtonToggle()
{
  // read in the current state of the button
  button_state=digitalRead(BUTTON_PIN);
}

void setup()
{
  // add the driver pin
  pinMode(DRIVER_PIN, OUTPUT);
  // set the driver pin to LOW
  digitalWrite(DRIVER_PIN, LOW);
  
  // add the button pin
  pinMode(BUTTON_PIN, INPUT);
  // enable the pull-up register
  digitalWrite(BUTTON_PIN, HIGH);
  
  // Identify the routine and conditions for our interrupt. 
  attachInterrupt(INTERRUPT_NUM, handleButtonToggle, CHANGE); 
	
  // start serial port at 9600 bps:
  Serial.begin(9600);
  
  scan_servo.attach(SERVO_PIN); // attaches the servo pin to the servo object  
}

void loop()
{
  // value read from IR sensor (centimeters)
  int val; 
  // the posision of the servo (degrees)
  short pos;
                 
  Serial.println("Moving 0 to 180");
  for(pos = 0; pos < 180; pos += 5)  
  {                   
    // drive the servo to our new position    
    scan_servo.write(pos);
    // wait 15 ms for the servo to reach it's new position
    delay(15);

    // wait until the sensor reads, and get the results
    while (false == ir_sensor.readSensor(val))
    {
      delay(1); // sleep 1 ms; 
    }

    // if the button is pressed, read out the data
    if (BUTTON_ENABLE == button_state)
    {
      Serial.print(val);
      Serial.print(" cm at ");
      Serial.print(pos);
      Serial.println(" degrees");
    }
  } // 

  // now spin the other way
  Serial.println("Moving 180 to 0");
  for(pos = 180; pos>=0; pos-=5) 
  {                                
    // drive the servo to our new position    
    scan_servo.write(pos);
    // wait 15 ms for the servo to reach it's new position
    delay(15);

    // wait until the sensor reads, and get the results
    while (false == ir_sensor.readSensor(val))
    {
      delay(1); // sleep 1 ms; 
    }
    
    // if the button is pressed, read out the data
    if (BUTTON_ENABLE == button_state)
    {
      Serial.print(val);
      Serial.print(" cm at ");
      Serial.print(pos);
      Serial.println(" degrees");
    }
  } 
  
}

